#include "RGBDCamera_FB.h"

#include <iostream>
#include <fstream>
#include <sstream>
#include <math.h>

//OpenCV
#include "cv.h"
#include "highgui.h"

#include "RGBDFrame.cpp"
#include "KeyPointSet.cpp"
#include "KeyPoint.cpp"
#include "FeatureDescriptor.cpp"

void RGBDCamera_FB::print()
{
	printf("FB camera...\n");
};

void RGBDCamera_FB::store(string path)
{
	printf("store FB camera:%s\n",path.c_str());
};

void RGBDCamera_FB::load(string path)
{
	printf("load FB camera:%s\n",path.c_str());
};

RGBDCamera_FB::RGBDCamera_FB(){
	printf("created RGBDCamera_FB\n");
	focal_length_x = 525.0;
	focal_length_y = 525.0;
	optical_center_x = 319.5;
	optical_center_y = 319.5;
	depth_scaling = 1.0;
	factor = 1;
	camera_type = 1;
}
RGBDCamera_FB::~RGBDCamera_FB(){}
void RGBDCamera_FB::setCameraID(int i)
{
	if(i == 1){
		focal_length_x = 517.3;
		focal_length_y = 516.5;
		optical_center_x = 318.6;
		optical_center_y = 255.3;
		depth_scaling = 1.035;
		factor = 1;
	}else if(i == 2){
		focal_length_x = 520.9;
		focal_length_y = 510.0;
		optical_center_x = 325.1;
		optical_center_y = 247.7;
		depth_scaling = 1.031 ;
		factor = 1;
	}else{
		focal_length_x = 525.0;
		focal_length_y = 525.0;
		optical_center_x = 319.5;
		optical_center_y = 319.5;
		depth_scaling = 1.0;
		factor = 1;	
	}
}

void RGBDCamera_FB::setup(string path)
{
	using namespace std;
	printf("setup FB camera: %s\n",path.c_str());
	data_path 					= path;
	string rgb_txt	 			= path+"rgb.txt";
	string depth_txt 			= path+"depth.txt";
	string groundtruth_txt 		= path+"groundtruth.txt";
	string accelerometer_txt 	= path+"accelerometer.txt";

	string ** rgb_list_tmp 		= new string * 	[1000000];//Should be made dynamic...
	double * rgb_timings 		= new double 	[1000000];//Should be made dynamic...
	int rgb_counter 			= 0;
	
	string ** depth_list_tmp	= new string * 	[1000000];//Should be made dynamic...
	double * depth_timings 		= new double 	[1000000];//Should be made dynamic...
	int depth_counter 			= 0;

	
	string line;
	ifstream myfile (rgb_txt.c_str());
	if (myfile.is_open())
	{
		while ( myfile.good() )
		{
			getline (myfile,line);
			if(line.empty()){				}
			else if(line.at(0) == '#'){		printf("comment: %s\n",line.c_str());}
			else{
											string rgb_filename = path+line.substr(line.find(" ")+1,line.length()-line.find(" "));
											istringstream buffer1((line.substr(0,line.find("."))));
											istringstream buffer2((line.substr(line.find(".")+1,line.find(" ")-line.find("."))));
											int timing_large;
											int timing_small;
											buffer1 >> timing_large;
											buffer2 >> timing_small;
											double timing_tmp = (double)timing_large+((double)timing_small)/1000000.0f;
											rgb_list_tmp[rgb_counter]	= new string(rgb_filename);
											rgb_timings[rgb_counter]	= timing_tmp;
											rgb_counter++;
			}
		}
		myfile.close();
	}else{
		cout << "Unable to open rgb txt file:" << endl;
	}
	ifstream myfile2 (depth_txt.c_str());
	if (myfile2.is_open())
	{
		while ( myfile2.good() )
		{
			getline (myfile2,line);
			if(line.empty()){				}
			else if(line.at(0) == '#'){		printf("comment: %s\n",line.c_str());}
			else{
											string depth_filename = path+line.substr(line.find(" ")+1,line.length()-line.find(" "));
											istringstream buffer1((line.substr(0,line.find("."))));
											istringstream buffer2((line.substr(line.find(".")+1,line.find(" ")-line.find("."))));
											int timing_large;
											int timing_small;
											buffer1 >> timing_large;
											buffer2 >> timing_small;
											double timing_tmp = (double)timing_large+((double)timing_small)/1000000.0f;
											depth_list_tmp[depth_counter]	= new string(depth_filename);
											depth_timings[depth_counter]	= timing_tmp;
											depth_counter++;
			}
		}
		myfile2.close();
	}else{
		cout << "Unable to open depth txt file:" << endl;
	}
	printf("nr_rgb: %i, nr_depth: %i\n",rgb_counter,depth_counter);
	int * best_rgb 		= new int[rgb_counter];
	int * best_depth 	= new int[depth_counter];
	for(int i = 0; i < rgb_counter;i++)
	{
		float best = 99999999999;
		for(int j = 0; j < depth_counter;j++)
		{
			if(fabs(rgb_timings[i]-depth_timings[j]) < best)
			{
				best = fabs(rgb_timings[i]-depth_timings[j]);
				best_rgb[i] = j;
			}
		}
	}
	
	for(int i = 0; i < depth_counter;i++)
	{
		float best = 99999999999;
		for(int j = 0; j < rgb_counter;j++)
		{
			if(fabs(rgb_timings[j]-depth_timings[i]) < best)
			{
				best = fabs(rgb_timings[j]-depth_timings[i]);
				best_depth[i] = j;
			}
		}
	}
	
	int counter = 0;
	for(int i = 0; i < rgb_counter;i++)
	{
		if(i == best_depth[best_rgb[i]]){
			//printf("%i:%i --> %f\n",i,best_rgb[i],fabs(rgb_timings[i]-depth_timings[best_rgb[i]]));
			counter++;
		}
	}
	printf("counter: %i\n",counter);
	for(int i = 0; i < max(rgb_counter,depth_counter);i++)
	{
		if(i < rgb_counter){
			//printf("%i: rgb:%f\n",i,rgb_timings[i]);
		}
		if(i < depth_counter){
			//printf("%i: dep:%f\n",i,depth_timings[i]);
		}
	}

	rgb_list 	= new string * [counter];
	depth_list 	= new string * [counter];
	timings 	= new double   [counter];
	counter 	= 0;
	for(int i = 0; i < rgb_counter;i++)
	{
		if(i == best_depth[best_rgb[i]]){
			rgb_list[counter]	= rgb_list_tmp[counter];
			depth_list[counter]	= depth_list_tmp[counter];
			timings[counter] 	= (rgb_timings[i]+depth_timings[best_rgb[i]])/2.0;
			/*
			IplImage * rgb_img = cvLoadImage( rgb_list[counter]->c_str(), CV_LOAD_IMAGE_COLOR );
			IplImage * depth_img = cvLoadImage( depth_list[counter]->c_str(), CV_LOAD_IMAGE_COLOR );
			cvNamedWindow("rgb_pixel_image", CV_WINDOW_AUTOSIZE );
			cvShowImage("rgb_pixel_image", rgb_img);
			cvNamedWindow("depth_pixel_image", CV_WINDOW_AUTOSIZE );
			cvShowImage("depth_pixel_image", depth_img);
			if(counter > 0){
				cvWaitKey(1000*(timings[counter]-timings[counter-1]));
			}else{
				cvWaitKey(1000/30);
			}
			cvReleaseImage( &rgb_img );
			cvReleaseImage( &depth_img );
			*/
			counter++;
		}
	}
	nr_frames = counter;
	delete best_rgb;
	delete best_depth;
};

